/**
 * 
 */
package mapmaker.robot;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.concurrent.Semaphore;

import mapmaker.config.Config;
import mapmaker.log.Log;
import mapmaker.map.EnvironmentMap;
import mapmaker.map.MapCell;
import mapmaker.network.NetworkCommunication;
import mapmaker.pose.Pose;
import mapmaker.pose.Position;
import mapmaker.robot.engine.DifferentialDrive;
import mapmaker.robot.engine.DifferentialDriveCommand;
import mapmaker.robot.engine.DifferentialDriveFeedback;
import mapmaker.robot.engine.DifferentialDriveProperties;
import mapmaker.robot.sensors.Lasers;
import mapmaker.utils.ComparatorCellDist;
import mapmaker.utils.Utilities;
import net.sf.json.JSONObject;

import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math3.util.Pair;

/**
 * @author Acid Flow
 *
 */
public class Robot {

	private static final String LOG_TAG = Robot.class.getSimpleName();
	public static Semaphore SEMAPHORE_EXPLORATION = new Semaphore(1, true);
	public static ArrayList<MapCell> PATH;

	private DifferentialDrive _differentialDrive;
	private Lasers _lasers;
	private Pose _pose;
	private EnvironmentMap _map;
	private boolean _obstacleForward;

	//Constructors
	public Robot(){
		_pose = new Pose();
		updatePose();
		DifferentialDriveProperties dProperties = NetworkCommunication.initDifferentialDrivePropertiesFromServer();
		_differentialDrive = new DifferentialDrive(new DifferentialDriveCommand(0, 0), new DifferentialDriveFeedback(0, 0, true), dProperties, 0);
		_differentialDrive.getCommand().setProperties(dProperties);
		_lasers = new Lasers();
		_obstacleForward = false;
	}

	// SIMPLE GETTER AND SETTER

	public void setEnvironmentMap(EnvironmentMap m){
		_map = m;
	}

	public EnvironmentMap getEnvironmentMap(){
		return _map;
	}

	public Pose getPose(){
		updatePose();
		return _pose;
	}

	public DifferentialDrive getDifferentialDrive(){
		return _differentialDrive;
	}

	public Lasers getLasers(){
		return _lasers;
	}
	
	public void setObstableForward(boolean obstacleForward) {
		_obstacleForward = obstacleForward;
	}

	private void setSpeed(double linear, double angular){
		_differentialDrive.getCommand().setTargetLinearSpeed(linear);
		_differentialDrive.getCommand().setTargetAngularSpeed(angular);
	}


	// UPDATING VALUES FROM SERVER
	/**
	 * Retrieve robot pose from server
	 */
	public void updatePose(){
		JSONObject obj = NetworkCommunication.getRobotPositionFromServer();
		_pose.setPoseFromJSONObject(obj);
	}

	/**
	 * Retrieve the differential drive from the server
	 */
	public void updateDifferentialDrive(){
		JSONObject obj = NetworkCommunication.getRobotDifferentialDriveFromServer();
		_differentialDrive.setDifferentialDriveFromJSONObject(obj); 
	}


	/**
	 * Retrieve the lasers from the server
	 */
	public void updateLasers(){
		_lasers.getCurrentLasers();
	}

	// SETTERS TO SERVER SIDE


	/**
	 * Set speeds of the robot and send command request to the server
	 * @param linear desired linear speed
	 * @param angular desired angular speed
	 */
	public void setSpeedAndGo(double linear, double angular){
		setSpeed(linear, angular);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
	}

	/**
	 * Send a request to stop the robot 
	 */
	public void stop(){
		setSpeedAndGo(0,0);
		NetworkCommunication.postSpeedToServer(this.getDifferentialDrive().getCommand().toJSONObject());
	}
	
	/**
	 * Retrieve the position of the robot from the server
	 * @return a Position
	 */
	public Position getPosition() {
		updatePose();
		return _pose.getPosition();
	}


	public void halfTurn() {
		double angleSpeed = getDifferentialDrive().getProperties().getMaxAngularSpeed();
		setSpeedAndGo(0, angleSpeed);
		try {
			Thread.sleep((long) (Math.PI*1000 / angleSpeed));
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
		stop();
	}

	/**
	 * Explore and create the map
	 */
	public void createMap() {
		do{
			explore();	
		} while(gotoNextCell());
	}

	/**
	 * Use the laser sensors to explore the map
	 */
	public void explore() {
		try {
			SEMAPHORE_EXPLORATION.acquire();
			Lasers.LASERS_SEM.acquire();
			updateLasers();
			ArrayList<Double> echoes = _lasers.getLasersEchoes();
			Lasers.LASERS_SEM.release();
			double inc = _lasers.getLasersProperties().getAngleIncrement();
			double ang = _lasers.getLasersProperties().getStartAngle();
			Pose robot = getPose();
			for(int i = 0; i < echoes.size(); i++) {
				if(echoes.get(i) != null) {
					Position lastPos = _pose.convertPositionToMapCoordinateSystem(new Position(Math.cos(i*inc + ang) * echoes.get(i), Math.sin(i*inc + ang) * echoes.get(i), 0), robot);

					MapCell lastCell = _map.getCellAt(lastPos);
					if(lastCell != null) {
						if(echoes.get(i) < Config.MAX_LASER_RANGE) {
							lastCell.setOccupied();
							lastCell = _map.getCellAt(lastPos);
						}
					}
					setCellFreeInBetween(lastPos, lastCell);
				} else {
					Log.e(LOG_TAG, "ECHOES NULL : " + i);
				}
			}
			
			_map.getCellAt(getPosition()).setFree();
			SEMAPHORE_EXPLORATION.release();
		} catch (InterruptedException e) {
			SEMAPHORE_EXPLORATION.release();
			e.printStackTrace();
		}
		_map.generateMapImage("test.gif");
	}
	
	private void setCellFreeInBetween(Position lastPos, MapCell lastCell) {
		Position robot = getPosition();

		double minSize = _map.getMinSize();
		double distanceX = (lastPos.getX() - robot.getX());
		double distanceY = (lastPos.getY() - robot.getY());
		int nbPas = (int) Math.abs((distanceX / minSize));
		double dx = Math.signum((distanceX / minSize)) * minSize;
		double dy = distanceY / nbPas;

		double vx, vy;
		MapCell cell;
		for(vx = 0, vy = 0; Math.abs(vx) < Math.abs(distanceX) && Math.abs(vy) < Math.abs(distanceY); vx += dx, vy += dy){
			cell = _map.getCellAt(new Position(robot.getX() + vx, robot.getY() + vy, 0));
			if(cell != null) {
				if(cell.getWeight() > 12) {
					break;
				} else {
					cell.setFree();
				}
			}
		}

	}

	
	/**
	 * Get a goal and try to go to it
	 * @return
	 */
	public boolean gotoNextCell(){
		try {
			SEMAPHORE_EXPLORATION.acquire();
			MapCell goal = getGoal();
			if(goal == null) {
				if(_map.getMinUnSeenDepth() <= Config.DEPTH_MAX) {
					_map.setMinUnSeenDepth();
					Log.i(LOG_TAG, "UNSEEN DEPTH : " + _map.getMinUnSeenDepth());
					SEMAPHORE_EXPLORATION.release();
					return true;
				} else {
					Log.i(LOG_TAG, "No more goal ");
					SEMAPHORE_EXPLORATION.release();
					return false;
				}
			}
			Log.e(LOG_TAG, "Goal : " + goal);
			PATH = getPath(goal);
			if(PATH == null) {
				Log.i(LOG_TAG, "Position : " + _map.getCellAt(getPosition()));
				goal.setUnreachable();
				Log.i(LOG_TAG, "Unreachable : " + goal);
				SEMAPHORE_EXPLORATION.release();
				return true;
			} else {
				PATH.remove(_map.getCellAt(getPosition()));
			}
			SEMAPHORE_EXPLORATION.release();
			int ret = 0;
			Log.i(LOG_TAG, "Position " + _map.getCellAt(getPosition()));
			for(MapCell cell : PATH) {
				Log.e(LOG_TAG, "Path : " + ((goal == cell) ? "goal " : " ") + cell);
				if(cell.isUnreachable()) {
					return true;
				} else if( cell.hasChildren()) {
					Log.e(LOG_TAG, "REDIVIDED : " + cell);
					return true;
				}
				ret = goTo(cell);
				if(ret != 1) {
					if(ret == -1) {
						Log.i(LOG_TAG, "Obstacle ");
						cell.setUnreachable();
						return true;
					}
				}
			}
		}catch (InterruptedException e) {
			SEMAPHORE_EXPLORATION.release();
			e.printStackTrace();
		}

		return true;
	}
	
	private MapCell getGoal() {
		Log.i(LOG_TAG, "Getting goal");
		MapCell current = _map.getBiggestCellAt(getPosition(), _map.getMap());
		MapCell goal = null;
		if((goal = findUndone(current)) != null) {
			return goal;
		}
		MapCell cell = current;
		for(int i = 0; i < _map.getNbRow(); i++) {
			for(int j = 0; j < _map.getNbCol(); j++) {
				cell = _map.getCellAt(i, j);
				if((goal = findUndone(cell)) != null) {
					return goal;
				}
			}
		}
		return null;
	}

	

	private MapCell findUndone(MapCell cell) {
		if(cell.hasChildren() && cell.getDepth() < _map.getMinUnSeenDepth()) {
			MapCell[][] children = cell.getChildren();
			MapCell ret = null;
			for(int i = 0; i < children.length; i++) {
				for(int j = 0; j < children[i].length; j++) {
					if((ret = findUndone(children[i][j])) != null) {
						return ret;
					}
				}
			}
			return null;
		} else {
			if(!cell.hasBeenSeen()) {
				return cell;
			} else {
				return null;
			}
		}
	}


	/**
	 * A* to get a path to the goal
	 * @param goal
	 * @return the path
	 */
	private ArrayList<MapCell> getPath(MapCell goal) {
		Log.i(LOG_TAG, "Path computing");
		ArrayList<MapCell> closed = new ArrayList<MapCell>();
		ArrayList<Pair<Double, MapCell> > open = new ArrayList<Pair<Double, MapCell>>();
		HashMap<MapCell, MapCell> path = new HashMap<MapCell, MapCell>();
		HashMap<MapCell, Double> from = new HashMap<MapCell, Double>();
		Position robot = getPosition();

		MapCell current = _map.getCellAt(robot);
		open.add(new Pair<Double, MapCell>(Utilities.norm(robot, goal.getMiddleCell()), current));
		from.put(current, 0.0);

		while(!open.isEmpty()) {
			current = open.get(0).getValue();
			if(current == goal) {
				Log.i(LOG_TAG, "End of computing");
				return buildPath(path, goal);
			}

			open.remove(0);
			closed.add(current);
			ArrayList<MapCell> neighbors = _map.expand(current);
			for(MapCell cell : neighbors) {
				if(!closed.contains(cell)) {

					double score = from.get(current) + Utilities.norm(robot, cell.getMiddleCell());

					if(!open.contains(cell) || score < from.get(cell)) {
						path.put(cell, current);
						from.put(cell, score);
						open.add(new Pair<Double, MapCell>(score + Utilities.norm(cell.getMiddleCell(), goal.getMiddleCell()), cell));
					}
					closed.add(cell);
				}
			}
			Collections.sort(open, new ComparatorCellDist());
		}
		Log.i(LOG_TAG, "End of computing : null");
		return null;
	}

	private ArrayList<MapCell> buildPath(HashMap<MapCell, MapCell> path, MapCell goal) {
		ArrayList<MapCell> p = null;
		if(path.containsKey(goal)) {
			p = buildPath(path, path.get(goal));
		} else {
			p = new ArrayList<MapCell>();
		}
		p.add(goal);
		return p;
	}

	/**
	 * Go to a cell using follow the carrot like
	 * @param goal
	 * @return
	 */
	public int goTo(MapCell goal) {
		int ret;
		rotateToGoal(goal, 2);
		goForwardToGoal(goal, 1, 10);
		if(_obstacleForward){
			_obstacleForward = false;
			ret = -1; 
		} else{
			ret = 1;
		}
		return ret;
	}


	private void rotateToGoal(MapCell goal, double speedDiviser){
		try {
			SEMAPHORE_EXPLORATION.acquire();
			double angle = getAngleToGoal(goal);
			setSpeedAndGo(0, Math.signum(angle) *_differentialDrive.getProperties().getMaxAngularSpeed() / speedDiviser);
			while(Math.abs(angle) > Math.toRadians(Config.ERROR_ANGLE)){
				angle = getAngleToGoal(goal);
			}
			stop();
			SEMAPHORE_EXPLORATION.release();
		} catch (InterruptedException e) {
			e.printStackTrace();
		}
	}

	private boolean goForwardToGoal(MapCell goal, double linearSpeedDiviser, double angularSpeedDiviser){
		double angle = getAngleToGoal(goal);
		double distance = getDistanceToGoal(goal);
		setSpeedAndGo(_differentialDrive.getProperties().getMaxLinearSpeed() / linearSpeedDiviser, 0);
		while(distance > Config.ERROR_IS_ARRIVED && !_obstacleForward){
			distance = getDistanceToGoal(goal);
			angle = getAngleToGoal(goal);
			if(Math.abs(angle) > Math.toRadians(Config.ERROR_ANGLE)){
				stop();
				rotateToGoal(goal, angularSpeedDiviser);
				setSpeedAndGo(_differentialDrive.getProperties().getMaxLinearSpeed() / linearSpeedDiviser, 0);
			}
		}
		stop();
		return _obstacleForward;
	}
	
	public boolean isObstacleForward(){
		try {
			Lasers.LASERS_SEM.acquire();
		} catch (InterruptedException e) {
			Lasers.LASERS_SEM.release();
			e.printStackTrace();
		}
		updateLasers();
		ArrayList<Double> lasers = _lasers.getLasersEchoes();
		Lasers.LASERS_SEM.release();
		for(int i = 90; i < 180; i++){
			if(lasers.get(i) < Config.DISTANCE_STOP_OBSTACLE){
				Lasers.LASERS_SEM.release();
				return true;
			}
		}
		
		return false;
	}
	
	private Vector3D convertGoalToRobotCoordinateSystem(MapCell goal){
		Position centerGoal = goal.getMiddleCell();
		return Pose.convertPositionToRobotCoordinateSystem(this, centerGoal);
	}

	private double getAngleToGoal(MapCell goal){
		Vector3D goalPoint = convertGoalToRobotCoordinateSystem(goal);
		return Math.atan2(goalPoint.getY(), goalPoint.getX());
	}

	private double getDistanceToGoal(MapCell goal){
		Vector3D goalPoint = convertGoalToRobotCoordinateSystem(goal);
		return goalPoint.getNorm();
	}
}
